/*
 * @Description: 卡尔曼滤波器
 * @Author: Dryad
 * @Date: 2019-06-17 21:16:17
 */
#pragma once
#include <arm_math.h>

/* 卡尔曼滤波见Kalman.md文件 */
namespace Kalman_Filter_Function
{
/* 根据t-1时刻的状态量和t时刻控制量预测t时刻状态预测量 */
void Forecast_State_Variable(const arm_matrix_instance_f32 &A,           /* 状态转换矩阵A */
                             const arm_matrix_instance_f32 &sys_state,   /* t-1时刻的状态量 */
                             const arm_matrix_instance_f32 &B,           /* 控制转换矩阵B */
                             const arm_matrix_instance_f32 &input,       /* 控制量 */
                             arm_matrix_instance_f32 &sys_state_forecast /* t时刻的状态预测量 */
);

/* 更新预测协方差矩阵 */
void Forecast_Covariance(const arm_matrix_instance_f32 A,             /* 状态转换矩阵A */
                         const arm_matrix_instance_f32 &covariance,   /* t-1时刻的协方差矩阵 */
                         const arm_matrix_instance_f32 &R,            /* 控制噪声矩阵R */
                         arm_matrix_instance_f32 &covariance_forecast /* t时刻的协方差预测矩阵 */
);

/* 计算卡尔曼增益矩阵 */
void Cal_Kalman_Gain(const arm_matrix_instance_f32 &covariance_forecast, /* t时刻的协方差预测矩阵 */
                     const arm_matrix_instance_f32 &C,                   /* 测量转换矩阵C */
                     const arm_matrix_instance_f32 &Q,                   /* 测量噪声矩阵Q */
                     arm_matrix_instance_f32 &K                          /* 卡尔曼增益矩阵 */
);

/* 使用测量值更新状态量 */
void Update_State_Variable(const arm_matrix_instance_f32 &sys_state_forecast, /* t时刻的状态预测量 */
                           const arm_matrix_instance_f32 &K,                  /* 卡尔曼增益矩阵 */
                           const arm_matrix_instance_f32 &measurement,        /* t时刻测量量 */
                           const arm_matrix_instance_f32 &C,                  /* 测量转换矩阵C */
                           arm_matrix_instance_f32 &sys_state                 /* t时刻的状态量 */
);

/* 更新协方差 */
void Update_Covariance(const arm_matrix_instance_f32 &covariance_forecast, /* t时刻的协方差预测矩阵 */
                       const arm_matrix_instance_f32 &I,                   /* 单位矩阵 */
                       const arm_matrix_instance_f32 &K,                   /* 卡尔曼增益矩阵 */
                       const arm_matrix_instance_f32 &C,                   /* 测量转换矩阵C */
                       arm_matrix_instance_f32 &covariance                 /* t时刻的协方差矩阵 */
);

/* A为单位矩阵时，根据t-1时刻的状态量和t时刻控制量预测t时刻状态预测量 */
void Forecast_State_Variable(const arm_matrix_instance_f32 &sys_state,   /* t-1时刻的状态量 */
                             const arm_matrix_instance_f32 &B,           /* 控制转换矩阵B */
                             const arm_matrix_instance_f32 &input,       /* 控制量 */
                             arm_matrix_instance_f32 &sys_state_forecast /* t时刻的状态预测量 */
);

/* A为单位矩阵时，更新预测协方差矩阵 */
inline void Forecast_Covariance(const arm_matrix_instance_f32 &covariance,   /* t-1时刻的协方差矩阵 */
                                const arm_matrix_instance_f32 &R,            /* 控制噪声矩阵R */
                                arm_matrix_instance_f32 &covariance_forecast /* t时刻的协方差预测矩阵 */
)
{
    arm_mat_add_f32(&covariance, &R, &covariance_forecast); /* 计算协方差预测矩阵 */
}

/* C为单位矩阵时，计算卡尔曼增益矩阵 */
void Cal_Kalman_Gain(const arm_matrix_instance_f32 &covariance_forecast, /* t时刻的协方差预测矩阵 */
                     const arm_matrix_instance_f32 &Q,                   /* 测量噪声矩阵Q */
                     arm_matrix_instance_f32 &K                          /* 卡尔曼增益矩阵 */
);

/* C为单位矩阵时，使用测量值更新状态量 */
void Update_State_Variable(const arm_matrix_instance_f32 &sys_state_forecast, /* t时刻的状态预测量 */
                           const arm_matrix_instance_f32 &K,                  /* 卡尔曼增益矩阵 */
                           const arm_matrix_instance_f32 &measurement,        /* t时刻测量量 */
                           arm_matrix_instance_f32 &sys_state                 /* t时刻的状态量 */
);

/* C为单位矩阵时，更新协方差 */
void Update_Covariance(const arm_matrix_instance_f32 &covariance_forecast, /* t时刻的协方差预测矩阵 */
                       const arm_matrix_instance_f32 &I,                   /* 单位矩阵 */
                       const arm_matrix_instance_f32 &K,                   /* 卡尔曼增益矩阵 */
                       arm_matrix_instance_f32 &covariance                 /* t时刻的协方差矩阵 */
);

/* 计算正定矩阵 output=A*B*A' */
void Cal_A_mul_B_A_Trans(const arm_matrix_instance_f32 &A, const arm_matrix_instance_f32 &B, arm_matrix_instance_f32 &output);

} // namespace Kalman_Filter_Function
